# -*- coding = utf-8 -*-
# @Time : 2023/3/21 1:52
# @Author : 龙王赘婿_彪少
# @File : standard_robat_screw.py
# @Software: PyCharm

# 功能：pc200 丝杆控制程序
# 型号：和利时LS

import serial
import modbus_tk.defines as cst
from modbus_tk import modbus_rtu

PORT = "com10"
baudrate = 9600
ID = 0x01

BASE_ADDR = 0x3A98  # 写不保存
BASE_ADDR_SAVE = 0x2710  # 写保存

# 丝杆方向 右手方向，靠近，载荷向下运动
# Fn004 = 0 右手方向为正，即是向下为正，即是向上归零
GEER = 200  # 丝杆导程 rp meter
MAX_SPEED = 0.2  # 丝杆最大速度 mps
CENT = 4096  # 电机线数


class Modbus:
    def __init__(self):
        self.master = modbus_rtu.RtuMaster(
            serial.Serial(port=PORT, baudrate=baudrate, bytesize=8, parity='N', stopbits=1, xonxoff=0))
        self.master.set_timeout(1.0)

    def read_holding_resasters(self, addr, number) -> tuple:
        try:
            res = self.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=addr, quantity_of_x=number)
            return res
        except Exception as err:
            print("异常：", err)

    def write_single_register(self, addr, value):
        try:
            res = self.master.execute(ID, cst.WRITE_SINGLE_REGISTER, starting_address=addr, output_value=value)
            # print(res)
        except Exception as err:
            print("异常：", err)

    def write_mutiple_registers(self, addr, value_tuple):
        try:
            # print(value_tuple, type(value_tuple))
            res = self.master.execute(ID, cst.WRITE_MULTIPLE_REGISTERS, starting_address=addr, output_value=value_tuple)
            # print(res)
        except Exception as err:
            print("异常：", err)

    def screw_init(self):
        # 配置应用模式Fn000 为2 选择modbus
        self.write_single_register(BASE_ADDR_SAVE + 0x000, 0x0002)
        # 配置总线控制模式Fn003 为6 绝对位置立即模式
        self.write_single_register(BASE_ADDR_SAVE + 0x003, 0x0006)
        # 配置电机旋转方向Fn004 为1 反向
        # self.write_single_register(BASE_ADDR_SAVE + 0x004, 0x0001)
        # 配置限位逻辑取反 Fn00E
        self.write_single_register(BASE_ADDR_SAVE + 0x00E, 11)
        # 配置电机使能为0 Fn010 因为如果是-1 的话，会与限位冲突
        self.write_single_register(BASE_ADDR_SAVE + 0x010, 0x0000)
        # 配置该值为0 Fn011 因为如果是-1 的话，会与限位冲突
        self.write_single_register(BASE_ADDR_SAVE + 0x011, 0x0000)
        # 配置正转限位 Fn01E  -1 sin1
        self.write_single_register(BASE_ADDR_SAVE + 0x01E, 0xFFFF)
        # 配置反转限位 Fn01F  -2 sin2
        self.write_single_register(BASE_ADDR_SAVE + 0x01F, 0xFFFE)
        # 使能正反转检测 Fn02F
        self.write_single_register(BASE_ADDR_SAVE + 0x02F, 0x0001)
        # 配置485模式（没啥用） Fn0F0
        # self.write_single_register(BASE_ADDR_SAVE + 0x0F0, 0x0001)
        # 配置波特率 Fn0F1 96
        # self.write_single_register(BASE_ADDR_SAVE + 0x0F1, 96)
        # 配置节点地址 Fn0F2
        # self.write_single_register(BASE_ADDR_SAVE + 0x0F2, 0x0001)
        # 归零时间窗配置为0 Fn1A7
        self.write_single_register(BASE_ADDR_SAVE + 0x1A7, 0x0001)

    def screw_enable(self):
        # 使能驱动器Fn010
        self.write_single_register(BASE_ADDR + 0x010, 0x0001)

    def screw_disable(self):
        # 禁能驱动器Fn010
        self.write_single_register(BASE_ADDR + 0x010, 0x0000)

    # def screw_goto_zero(self):
    #     speed = 0x64
    #     # 配置参考点到目标原点距离 圈数 Fn1A3
    #     self.write_single_register(BASE_ADDR_SAVE + 0x1A3, 0x0000)
    #     # 配置参考点到目标原点距离 单圈脉冲数 Fn1A4
    #     self.write_single_register(BASE_ADDR_SAVE + 0x1A4, 0x0000)
    #     # 原点回归速度（第一速度）RPM Fn1A5
    #     self.write_single_register(BASE_ADDR_SAVE + 0x1A5, speed)
    #     # 原点回归速度（第二速度）RPM Fn1A6
    #     self.write_single_register(BASE_ADDR_SAVE + 0x1A6, speed)
    #     # 原点回归找参考点确认时间窗 Fn1A7
    #     self.write_single_register(BASE_ADDR_SAVE + 0x1A7, 0x0000)
    #     # 配置执行归零 Fn020
    #     self.write_single_register(BASE_ADDR + 0x020, 0x0000)
    #     self.write_single_register(BASE_ADDR + 0x020, 0x0001)

    def screw_stop(self):
        self.screw_disable()

    def screw_position_move_absolute(self, speed_mps, position_m):
        if speed_mps <= 0.0:
            speed_mps = 0.0
        elif speed_mps >= MAX_SPEED:
            speed_mps = MAX_SPEED
        # 运行时间 ms
        position_now = self.screw_get_position()
        print("position_now ", position_now)
        position_delta = abs(position_m - position_now)
        runtime = int(position_delta * 1000 / speed_mps)
        # 计算位置 目标刻度 = 圈数 * 每圈刻度
        position = int(position_m * GEER * CENT)
        print(f"目标位置 = {position_m},刻度 = {position}")
        position_L = position & 0xffff
        position_H = (position >> 16) & 0xffff
        self.write_mutiple_registers(0x55F0, (runtime, position_L, position_H))

    # def screw_position_move_cent(self):
    #     # 电机转速 rpm = speed_mps * GEER * 60 单位
    #     speed = int(0.05 * GEER * 60)
    #     # 计算位置 目标刻度 = 圈数 * 每圈刻度
    #     position = int(CENT)
    #     print(f"目标位置 = 一圈,刻度 = {position}")
    #     position_L = position & 0xffff
    #     position_H = (position >> 16) & 0xffff
    #     self.write_mutiple_registers(0x55F0, (speed, position_L, position_H))

    def screw_get_position(self) -> int:
        position = m.read_holding_resasters(0x001D, 2)
        position_cent = position[0] + (position[1] << 16)
        position_meter = position_cent / (CENT * GEER)
        print(f"当前位置 = {position_meter},刻度 = {position_cent}")
        return position_cent

    # def screw_goto_zero_seccessful(self) -> int:
    #     state = m.read_holding_resasters(0x005C, 1)
    #     return (state[0] >> 15) & 0x1

    def screw_get_limit(self) -> int:
        state = m.read_holding_resasters(0x0030, 1)
        return state[0]

    def screw_get_positive_limit(self) -> int:
        return self.screw_get_limit() & 0x1

    def screw_get_negative_limit(self) -> int:
        return (self.screw_get_limit() >> 1) & 0x1


if __name__ == "__main__":
    m = Modbus()
    while True:
        key = input("输入指令：")
        if key == "e":
            m.screw_enable()
        elif key == "d":
            m.screw_disable()
        # elif key == "z":
        #     m.screw_goto_zero()
        # elif key == "zs":
        #     print(m.screw_goto_zero_seccessful())
        elif key == "p":
            m.screw_get_position()
        elif key == "i":
            m.screw_init()
        # elif key == "c":
        #     m.screw_position_move_cent()
        elif key == "l":
            print(m.screw_get_limit())
        elif key == "0":
            m.screw_position_move_absolute(0.05, 0.0)
        elif key == "1":
            m.screw_position_move_absolute(0.05, 0.1)
        elif key == "2":
            m.screw_position_move_absolute(0.05, 0.2)
        elif key == "3":
            m.screw_position_move_absolute(0.05, 0.3)
        elif key == "4":
            m.screw_position_move_absolute(0.05, 0.4)
        elif key == "5":
            m.screw_position_move_absolute(0.05, 0.5)
        elif key == "6":
            m.screw_position_move_absolute(0.05, 0.6)
        elif key == "7":
            m.screw_position_move_absolute(0.05, 0.7)
        elif key == "8":
            m.screw_position_move_absolute(0.05, 0.8)
        elif key == "9":
            m.screw_position_move_absolute(0.05, 0.9)
        elif key == "10":
            m.screw_position_move_absolute(0.05, 1)
        elif key == "100":
            m.screw_position_move_absolute(0.05, 10)
